//You need to program this file.

#include "../NoEdit/ProcessorMulti_Processor_Core_PrivFunc.h"
#include <cmath>

//*******************Please add static libraries in .pro file*******************
//e.g. unix:LIBS += ... or win32:LIBS += ...

bool DECOFUNC(setParamsVarsOpenNode)(QString qstrConfigName, QString qstrNodeType, QString qstrNodeClass, QString qstrNodeName, void * paramsPtr, void * varsPtr)
{
	XMLDomInterface xmlloader(qstrConfigName,qstrNodeType,qstrNodeClass,qstrNodeName);
	ProcessorMulti_Processor_Core_Params * params=(ProcessorMulti_Processor_Core_Params *)paramsPtr;
	ProcessorMulti_Processor_Core_Vars * vars=(ProcessorMulti_Processor_Core_Vars *)varsPtr;
	/*======Please Program below======*/
	/*
	Function: open node.
	Procedure:
	1: load parameters (params). [GetParamValue(xmlloader,params,tag); GetEnumParamValue(xmlloader,params,tag); GetUEnumParamValue(xmlloader,params,tag)]
	2: initialize variables (vars).
	3: If everything is OK, return 1 for successful opening and vice versa.
	*/
	
	return 1;
}

bool DECOFUNC(handleVarsCloseNode)(void * paramsPtr, void * varsPtr)
{
	ProcessorMulti_Processor_Core_Params * params=(ProcessorMulti_Processor_Core_Params *)paramsPtr;
	ProcessorMulti_Processor_Core_Vars * vars=(ProcessorMulti_Processor_Core_Vars *)varsPtr;
	/*======Please Program below======*/
	/*
	Function: close node.
	Procedure:
	1: handle/close variables (vars).
	2: If everything is OK, return 1 for successful closing and vice versa.
	*/
	
	return 1;
}

void DECOFUNC(getInternalTrigger)(void * paramsPtr, void * varsPtr, QObject * & internalTrigger, QString & internalTriggerSignal)
{
	ProcessorMulti_Processor_Core_Params * params=(ProcessorMulti_Processor_Core_Params *)paramsPtr;
	ProcessorMulti_Processor_Core_Vars * vars=(ProcessorMulti_Processor_Core_Vars *)varsPtr;
	internalTrigger=NULL;
	internalTriggerSignal=QString();
	/*======Occasionally Program above======*/
	/*
	Function: get internal trigger [defined in vars] for node.
	You need to program here when you need internal trigger (internalTrigger + internalTriggerSignal) for node.
	E.g.
	internalTrigger=&(vars->trigger);
	internalTriggerSignal=QString(SIGNAL(triggerSignal()));
	*/
}

void DECOFUNC(initializeOutputData)(void * paramsPtr, void * varsPtr, boost::shared_ptr<void> & outputDataPtr)
{
	ProcessorMulti_Processor_Core_Params * params=(ProcessorMulti_Processor_Core_Params *)paramsPtr;
	ProcessorMulti_Processor_Core_Vars * vars=(ProcessorMulti_Processor_Core_Vars *)varsPtr;
	outputDataPtr=boost::shared_ptr<void>(new SourceDrainMono_Sensor_EncoderIMU_Order_InputData());
	/*======Occasionally Program below/above======*/
	/*
	Function: initial output data.
	You need to program here when you need to manually initialize output data.
    */
}

void DECOFUNC(getMultiInputDataSize)(void * paramsPtr, void * varsPtr, QList<int> & inputDataSize)
{
	ProcessorMulti_Processor_Core_Params * params=(ProcessorMulti_Processor_Core_Params *)paramsPtr;
	ProcessorMulti_Processor_Core_Vars * vars=(ProcessorMulti_Processor_Core_Vars *)varsPtr;
	inputDataSize=QList<int>();
	/*======Please Program above======*/
	/*
	Function: get input data size to be grabbed from buffer.
	Rules:
	inputDataSize=0: grab and remove all data from buffer.
	inputDataSize>0: grab inputDataSize latest data from buffer.
	inputDataSize<0: grab and remove inputDataSize ancient data from buffer.
	E.g.
	inputDataSize=QList<int>()<<0<<1<<-1...;
	*/
    inputDataSize=QList<int>()<<10<<10<<10;
}

void behaviorCruise(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed);
void behaviorCorner1(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed);
void behaviorCorner2(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed);
void behaviorCorner3(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed);
void behaviorCorner4(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed);
void behaviorCorner5(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed);
void behaviorSbend(ProcessorMulti_Processor_Core_Vars *vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *> &inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *> &inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *> &inputdata_2, short &steer, short &speed);
void behaviorAvoid(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed);

//Input Port #0: Buffer_Size = 10, Params_Type = SourceDrainMono_Sensor_EncoderIMU_Params, Data_Type = SourceDrainMono_Sensor_EncoderIMU_Data
//Input Port #1: Buffer_Size = 10, Params_Type = SensorTimer_Sensor_URG_Params, Data_Type = SensorTimer_Sensor_URG_Data
//Input Port #2: Buffer_Size = 10, Params_Type = SensorTimer_Sensor_xtion_Params, Data_Type = SensorTimer_Sensor_xtion_Data
bool DECOFUNC(processMultiInputData)(void * paramsPtr, void * varsPtr, QVector<QVector<void *> > inputParams, QVector<QVector<void *> > inputData, void * outputData, QList<int> & outputPortIndex)
{
	ProcessorMulti_Processor_Core_Params * params=(ProcessorMulti_Processor_Core_Params *)paramsPtr;
	ProcessorMulti_Processor_Core_Vars * vars=(ProcessorMulti_Processor_Core_Vars *)varsPtr;
	QVector<SourceDrainMono_Sensor_EncoderIMU_Params *> inputparams_0; copyQVector(inputparams_0,inputParams[0]);
	QVector<SensorTimer_Sensor_URG_Params *> inputparams_1; copyQVector(inputparams_1,inputParams[1]);
	QVector<SensorTimer_Sensor_xtion_Params *> inputparams_2; copyQVector(inputparams_2,inputParams[2]);
	QVector<SourceDrainMono_Sensor_EncoderIMU_Data *> inputdata_0; copyQVector(inputdata_0,inputData[0]);
	QVector<SensorTimer_Sensor_URG_Data *> inputdata_1; copyQVector(inputdata_1,inputData[1]);
	QVector<SensorTimer_Sensor_xtion_Data *> inputdata_2; copyQVector(inputdata_2,inputData[2]);
	SourceDrainMono_Sensor_EncoderIMU_Order_InputData * outputdata=(SourceDrainMono_Sensor_EncoderIMU_Order_InputData *)outputData;
	outputPortIndex=QList<int>();
	if(inputdata_0.size()==0){return 0;}
	if(inputdata_1.size()==0){return 0;}
	if(inputdata_2.size()==0){return 0;}
	/*======Please Program below======*/
	/*
	Step 1: process inputdata_index, then store it into outputdata.
	Step 2 [optional]: determine the outputPortIndex. (if not, outputdata will be sent by all ports)
	E.g. outputPortIndex=QList<int>()<<(outportindex1)<<(outportindex2)...
	*/

    //inputdata_0                                             // EncoderIMU
    //inputdata_1                                             // URG
    //inputdata_2                                             // Xtion (RGB && depth)
    //cv::imshow("color", inputdata_2.front()->cvColorImg);   // Show RGB image
    //cv::imshow("depth", inputdata_2.front()->cvDepthImg);   // Show depth image

    short steer = 0;           // [-400, 400]
    short speed = 0;           // [-180, 180]

    if(inputdata_0.size() >= 10 && inputdata_1.size() >= 10 && inputdata_2.size() >= 10) {
        behaviorCruise(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
        behaviorCorner1(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
        behaviorCorner2(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
        behaviorCorner3(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
        behaviorCorner4(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
        behaviorCorner5(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
        behaviorSbend(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
        behaviorAvoid(vars, inputdata_0, inputdata_1, inputdata_2, steer, speed);
    }
    else {
        vars->startTime = inputdata_0[0]->qtimestamp;
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }

    int maxSpeed = 180;
    if (speed > maxSpeed) speed = maxSpeed;
    if (speed < -maxSpeed) speed = -maxSpeed;
    // Show RGB image && compass
    double ori = - ((double)steer / 400.0) * (M_PI / 2.0);
    cv::Mat img(350, 250, CV_AA, cv::Scalar(32, 32, 32));
    //inputdata_2.front()->cvColorImg.copyTo(img);
    //cv::flip(img, img, 1);
    cv::Point compass = cv::Point(100, 100);
    cv::circle(img, compass, 80, cv::Scalar(0,255,0), 1, CV_AA);
    cv::line(img, compass,
             cv::Point(compass.x - sin(ori) * 80,
                       compass.y - cos(ori) * 80),
             cv::Scalar(0,255,0), 3, CV_AA);
    QString str;
    str.sprintf("sp:%d", speed);
    cv::putText(img, str.toStdString(), cv::Point(60, 135), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255));
    str.sprintf("state:%d", vars->state);
    cv::putText(img, str.toStdString(), cv::Point(30, 250), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255));
    str.sprintf("dis:%.1f", (inputdata_0[0]->odometry - vars->lastOdemetry));
    cv::putText(img, str.toStdString(), cv::Point(30, 300), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255));
    //str.sprintf("x:%d y:%d", (int)(inputdata_0[0]->x / 1e3), (int)(inputdata_0[0]->y / 1e3));
    //cv::putText(img, str.toStdString(), cv::Point(30, 250), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 0, 0));
    cv::imshow("color", img);

    //--------------------------------------------
    char dataput[20];
    dataput[0] = 0xF8;
    dataput[1] = 4;
    *(short*)&dataput[2] = (short)steer;
    *(short*)&dataput[4] = (short)speed;
    dataput[6] = 0x8F;
    outputdata->datagram = QByteArray(dataput, 7);
	return 1;
}

int calcAverageDis(const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, int start, int end, int eps = 5)
{
    int ave_dis = 0;
    int enf_cnt = 0;
    for (int i = start; i < end; i++) {
        if (inputdata_1.front()->data[i] > eps) {
            ave_dis += inputdata_1.front()->data[i];
            enf_cnt++;
        }
    }
    if (enf_cnt > 0) {
        ave_dis /= enf_cnt;
    }
    return ave_dis;
}

void behaviorCruise(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    const int MAX_DIS = 100;
    const int NORMAL_SPEED = 150;

    speed = NORMAL_SPEED;

    if (vars->state != line1 &&
            vars->state != line2 &&
            vars->state != line3 &&
            vars->state != line4 &&
            vars->state != line5 &&
            vars->state != line6 &&
            vars->state != sbend && 
            vars->state != barrierLeft &&
            vars->state != barrierRight) 
        return;
    double steerp = 0, steerd = 0, steeri = 0;
    double p = -8, i = 0, d = 2.5;
    if (vars->state == line1)
        p = -10;
    
    for (int i = 0; i < 90; i++) {
        int p = (std::min((int)inputdata_1.front()->data[361-i], MAX_DIS) - std::min((int)inputdata_1.front()->data[i], MAX_DIS));
        int d = p - (std::min((int)inputdata_1[1]->data[361-i], MAX_DIS) - std::min((int)inputdata_1[1]->data[i], MAX_DIS));
        
        steerp += p / 90.0;

        steerd += d / 90.0;
    }
    double ix=0.5;
    steeri = steerp + steeri*ix;

    steer = p * steerp + i * steeri + d * steerd;
}
//该函数实现了第一个弯道的行为
void behaviorCorner1(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    int ave_dis = calcAverageDis(inputdata_1, 30, 60);
    if (vars->state == line1) {
        if (ave_dis > 250 && inputdata_0[0]->odometry - vars->lastOdemetry > 30) {
            vars->state = corner1;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }

    if (vars->state == corner1) {
        steer += 400;
        if (ave_dis < 200) {
            vars->state = line2;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }
}
//该函数实现了第二个弯道的行为
void behaviorCorner2(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    int ave_dis = calcAverageDis(inputdata_1, 30, 60);
    if (vars->state == line2) {
        if (ave_dis > 250 ){// && inputdata_0[0]->odometry - vars->lastOdemetry > 2000000) {
            vars->state = corner2;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }

    if (vars->state == corner2) {
        steer += 400;
        if (inputdata_0[0]->odometry - vars->lastOdemetry > 1.8) {
            vars->state = line3;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }
}
//该函数实现了第三个弯道的行为
void behaviorCorner3(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    int ave_dis_left = calcAverageDis(inputdata_1, 300, 330);
    int ave_dis_right = calcAverageDis(inputdata_1, 30, 60);
    if (vars->state == line3 && inputdata_0[0]->odometry - vars->lastOdemetry > 10) {
        if (ave_dis_left > 250) {
            vars->state = corner3Left;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }

    if (vars->state == corner3Left) {
        steer -= 300;
        if (ave_dis_left < 200 ) {
            vars->state = corner3Mid;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }

    if (vars->state == corner3Left || vars->state == corner3Mid) {
        if (ave_dis_right > 250) {
            vars->state = corner3Right;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }

    if (vars->state == corner3Right) {
        steer += 300;
        if (ave_dis_right < 200) {
            vars->state = line4;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }
}
//该函数实现了第四个弯道的行为
void behaviorCorner4(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    int ave_dis = calcAverageDis(inputdata_1, 30, 60);
    if (vars->state == line4 && inputdata_0[0]->odometry - vars->lastOdemetry > 7) {
        if (ave_dis > 250) {
            vars->state = corner4;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }

    if (vars->state == corner4) {
        steer += 250;
        if (inputdata_0[0]->odometry - vars->lastOdemetry > 3.5) {
            vars->state = line5;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }
}
//该函数实现了第五个弯道的行为
void behaviorCorner5(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    int ave_dis = calcAverageDis(inputdata_1, 30, 60);
    if (vars->state == line6) {
        if (ave_dis > 250) {
            vars->state = corner5;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }

    if (vars->state == corner5) {
        steer += 400;
        if (ave_dis < 200) {
            vars->state = line1;
            vars->lastOdemetry = inputdata_0[0]->odometry;
        }
    }
}
// void behaviorSbend(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
// {
//     if(vars->state == line5 && inputdata_0[0]->odometry - vars->lastOdemetry > 25){
//         vars->state = sbend;
//         steer +=200;
//         vars->lastOdemetry = inputdata_0[0]->odometry;
//     }
//     if(vars->state == sbend && inputdata_0[0]->odometry - vars->lastOdemetry > 5){
//         vars->state = line6;
//         vars->lastOdemetry = inputdata_0[0]->odometry;
//     }
// }
short barrierflag = 0;
void behaviorSbend(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    if(barrierflag>=4){
        vars->state = line6;
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }
    if(vars->state == line5 && inputdata_0[0]->odometry - vars->lastOdemetry > 21.5){
        vars->state = barrierLeft;
        barrierflag++;
        steer += 200;
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }
    if(vars->state == barrierLeft && inputdata_0[0]->odometry - vars->lastOdemetry > 2 ){
        vars->state = barrierRight;
        barrierflag++;
        steer -= 200;
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }
    if(vars->state == barrierRight && inputdata_0[0]->odometry - vars->lastOdemetry > 2){
        vars->state = barrierLeft;
        barrierflag++;
        steer += 200;
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }
}
/*
struct Sobstacle{
    short obstacle_right=10;
    short obstacle_left=350;
    bool isobstacle=false;
};
Sobstacle calObstacleDirection(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1)
{
    Sobstacle obstacle;
    short start = 20;
    short end = 340;
    for(int i=start;i<=end;i++){
        if(calcAverageDis(inputdata_1,i-20,i)-calcAverageDis(inputdata_1,i,i+20)>150){
            obstacle.obstacle_right=i;
        }
        if(calcAverageDis(inputdata_1,i,i+20)-calcAverageDis(inputdata_1,i-20,i)>150){
            obstacle.obstacle_left=i;
        }
    }
    if(obstacle.obstacle_left-obstacle.obstacle_right>=1&&obstacle.obstacle_left-obstacle.obstacle_right<=40){
        obstacle.isobstacle=true;
    }
//    for(int i=start;i<=end;i++){
//        int sum=0;
//        for(int j=0;j<10;j++){
//            sum+=inputdata_1.front()->data[i+j];
//        }
//        int ave=sum/10;
//        int var=0;
//        for(int j=0;j<10;j++){
//            var+=(inputdata_1.front()->data[i+j]-ave)*(inputdata_1.front()->data[i+j]-ave);
//        }
//        var/=10;
//        qDebug()<<"var"<<var<<endl;
//        if(var<10){
//            if(calcAverageDis(inputdata_1,i-10,i)-calcAverageDis(inputdata_1,i,i+10)>100){
//                obstacle.obstacle_right=i;
//            }
//            if(calcAverageDis(inputdata_1,i,i+10)-calcAverageDis(inputdata_1,i-10,i)>100){
//                obstacle.obstacle_left=i;
//            }
//        }
//    }
    if(obstacle.obstacle_left-obstacle.obstacle_right>=1&&obstacle.obstacle_left-obstacle.obstacle_right<=40){
        obstacle.isobstacle=true;
    }
    // qDebug()<<"obstacle_right"<<obstacle.obstacle_right<<"obstacle_left"<<obstacle.obstacle_left<<"isobstacle"<<obstacle.isobstacle<<endl;
    return obstacle;
}
void behaviorSbend(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
    short obstacle_dis = 1;
    Sobstacle obstacle;
    if(vars->state == line5 && inputdata_0[0]->odometry - vars->lastOdemetry > 20){
        obstacle = calObstacleDirection(vars, inputdata_1);
        if(obstacle.isobstacle == true){
            int ave_dis = calcAverageDis(inputdata_1, obstacle.obstacle_right, obstacle.obstacle_left);
            if(ave_dis < obstacle_dis){
                vars->state = barrierLeft;
                steer -= 200;
            }
        }
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }
    if(vars->state == barrierLeft){
        obstacle = calObstacleDirection(vars, inputdata_1);
        if(obstacle.isobstacle == true){
            vars->state = barrierRight;
            steer += 200;
        }
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }
    if(vars->state == barrierRight){
        obstacle = calObstacleDirection(vars, inputdata_1);
        if(obstacle.isobstacle == true){
            vars->state = barrierLeft;
            steer -= 200;
        }
        vars->lastOdemetry = inputdata_0[0]->odometry;
    }
	return;
}
*/
//该函数实现了避障功能
void behaviorAvoid(ProcessorMulti_Processor_Core_Vars * vars, const QVector<SourceDrainMono_Sensor_EncoderIMU_Data *>& inputdata_0, const QVector<SensorTimer_Sensor_URG_Data *>& inputdata_1, const QVector<SensorTimer_Sensor_xtion_Data *>& inputdata_2, short& steer, short& speed)
{
//     int min_dis = 1000000;
//     for (int i = 90; i < 270; i++) {
//         int ave_dis = 0;
//         int enf_cnt = 0;
//         for (int j = 0; j < 10; j++) {
//             if (inputdata_1.front()->data[i + j] > 5) {
//                 ave_dis += inputdata_1.front()->data[i + j];
//                 enf_cnt++;
//             }
//         }
//         if (enf_cnt > 0) {
//             ave_dis /= enf_cnt;
//             if (ave_dis < min_dis)
//                 min_dis = ave_dis;
//         }
//     }

//     const int INF_BACK = 2;
//     const int AVOID_DIS = 40;
//     int backspeed;
//     if (min_dis < AVOID_DIS) {
//         backspeed = 100;
//     }
//     else {
//         backspeed = INF_BACK * 100 / (min_dis - AVOID_DIS + INF_BACK);
//     }

//     speed -= backspeed;
}
